#include "AnimationQuatCurve.h"
#include "Common.h"
#include <glm/gtx/quaternion.hpp>
#include <cmath>

namespace DYGraphics
{
	bool IsEqual(glm::quat value1, glm::quat value2)
	{
		return abs(value1.x - value2.x) <= 0.0001f && abs(value1.y - value2.y) <= 0.0001f &&
			abs(value1.z - value2.z) <= 0.0001f && abs(value1.w - value2.w) <= 0.0001f;
	}

	void AnimationQuatCurve::AddKey(float pTime, glm::quat pValue)
	{
		AnimationQuatKey key = { pTime, pValue };
		if (mKeys.size() <= 1)
		{
			mKeys.push_back(key);
		}
		else
		{
			int endIdx = mKeys.size() - 1;
			if (IsEqual(pValue, mKeys[endIdx].value))
			{
				mKeys[endIdx].mTime = pTime;
			}
			else
			{
				mKeys.push_back(key);
			}
		}
	}

	glm::quat AnimationQuatCurve::Evaluate(float mTime)
	{
		if (mKeys.empty())
		{
			Print("AnimationVec3Curve::Evaluate has no key \n");
			return glm::vec3();
		}

		int size = mKeys.size();
		AnimationQuatKey startKey = mKeys[0];
		AnimationQuatKey stopKey = mKeys[size - 1];

		if (mTime < startKey.mTime || mTime > stopKey.mTime)
		{
			Print("AnimationVec3Curve::Evaluate mTime is invalid \n");
			return glm::vec3();
		}

		if (mTime == startKey.mTime) return startKey.value;
		if (mTime == stopKey.mTime) return stopKey.value;

		AnimationQuatKey preKey = startKey;
		for (int i = 1; i < mKeys.size(); i++)
		{
			AnimationQuatKey key = mKeys[i];
			if (mTime == key.mTime) return key.value;

			if (mTime < key.mTime)
			{
				return glm::lerp(preKey.value, key.value, (mTime - preKey.mTime) / (key.mTime - preKey.mTime));
			}

			preKey = key;
		}

		Print("AnimationVec3Curve::Evaluate ??? \n");
		return glm::vec3();
	}
}
